﻿/////////////////////////////////////////////////////////////////////////
//  WiimoteRecognizer.cs
//  Indoor Position Finder Application
//  Written by Francisco Vásquez (http://www.dcc.uchile.cl/~fvasquez)
//  Visit http://www.dcc.uchile.cl/~fvasquez/memoria
//  for more information.
/////////////////////////////////////////////////////////////////////////  
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using WiimoteLib;
using System.Diagnostics;
using TextToSpeech;
using System.IO;
using Socket;

namespace IndoorPositionFinder
{
    /// <summary>
    /// This class recognize the wiimotes connected with the
    /// computer and update the user position.
    /// Also provides information to the user using the TTS
    /// system
    /// </summary>
    public class WiimoteRecognizer
    {

        private WiimoteCollection mWC;
        // Check if this variable have any meaning
        private int wiimoteTotal;
        private SapiTTS notifier;
        
        // Create an association between the application ID and the persistant ID
        private Dictionary<Guid, String> wiimotesIDs;
        
        // Data obtained with the XML file
        private RoomDoor[] roomDoors;
        private PointF[] roomPoints;
        private RoomObject[] roomObjects;
        private Dictionary<String,RoomWiimote> roomWiimotes;
        private Dictionary<Guid,WiimoteState> wiimoteStates;
        private PointF lastUserPosition;

        // Saving the user position calculated by the application
        //private PointF userPosition;

        /// <summary>
        /// Initialize the WiimoteRecognizer class.
        /// </summary>
        /// <param name="room">Data obtained from the XML file</param>
        public WiimoteRecognizer(Room room)
        {
            // Collecting the room data
            this.roomDoors = room.Doors.ToArray();
            this.roomPoints = room.Points.ToArray();
            this.roomObjects = room.RoomObjects.ToArray();
            this.roomWiimotes = room.Wiimotes;
            this.wiimoteStates = new Dictionary<Guid, WiimoteState>();
            this.lastUserPosition = new PointF();

            wiimoteTotal = 0;
            notifier = new SapiTTS();
            wiimotesIDs = new Dictionary<Guid,String>();
            
            this.mWC = new WiimoteCollection();
            try
            {
                mWC.FindAllWiimotes();
            }
            catch (WiimoteNotFoundException ex)
            {
                Console.WriteLine("Wiimote not found error");
            }
            catch (WiimoteException ex)
            {
                Console.WriteLine("Wiimote Exception");
            }
            catch (Exception ex)
            {
                Console.WriteLine("Unknown error");
            }

            foreach (Wiimote wm in mWC)
            {
                //Connecting the wiimote
                String guid = wm.Connect();
                wm.SetReportType(InputReport.IRAccel, true);
                wm.SetLEDs(++wiimoteTotal);
                // Setting the association between the aplication ID and the persistant ID
                wiimotesIDs[wm.ID] = guid;
                Debug.WriteLine("Saving ID: "+guid+": into "+wm.ID);
                // Saving the Wiimote State for future request
                wiimoteStates.Add(wm.ID, wm.WiimoteState);
            }
        }
        /// <summary>
        /// Check if there is any change with the user position an update the data
        /// </summary>
        /// <param name="onlyFront">Determines what type of information need to be speak. True</param>
        /// <returns>The point associated with the new user position</returns>
        public System.Drawing.PointF changeChecker(bool onlyFront, bool device, int warningDistance = 20, String ip = "EOF")
        {
            int wiimoteChanged = 0;
            PointF[] position = new PointF[2];
            Guid[] wiimoteId = new Guid[2];
            bool updatePosition = false;
            foreach (KeyValuePair<Guid,WiimoteState> dictionary in wiimoteStates)
            {
                Guid tempGuid = dictionary.Key;
                WiimoteState ws = dictionary.Value;
                IRSensor irSensor = ws.IRState.IRSensors[0];
                if (irSensor.Found)
                {
                    wiimoteId[wiimoteChanged] = tempGuid;
                    position[wiimoteChanged++] = irSensor.Position;
                    // The update in the user position is produced only if
                    // at least two wiimotes can capture the user position
                    if (wiimoteChanged >= 2)
                    {
                        updatePosition = true;
                        break;
                    }
                }
            }
            // Here we put the logic required to warning to the user about any change
            // in his relation with the environment
            if (updatePosition)
            {
                Debug.WriteLine("Updating position");
                PointF oldPosition = this.lastUserPosition;
                // calculating the new user position
                this.lastUserPosition = this.userPosition(wiimoteId[0], wiimoteId[1], position[0], position[1]);
                // calculating the movement vector
                PointF movementVector = new PointF();
                movementVector.X = this.lastUserPosition.X - oldPosition.X;
                movementVector.Y = this.lastUserPosition.Y - oldPosition.Y;
                Debug.WriteLine(movementVector);
                if (!this.lastUserPosition.Equals(oldPosition) && movementVector.X > 0.3 && movementVector.Y > 0.3)
                {
                    movementVector.X = this.lastUserPosition.X - oldPosition.X;
                    movementVector.Y = this.lastUserPosition.Y - oldPosition.Y;
                    Debug.WriteLine(movementVector.ToString());

                    // calculating the centroid of the object and checking the distance between the user an
                    // the object's centroid. If the distance is smaller than warningDistance the program alert
                    // the user about the proximity
                    foreach (RoomObject roomObject in roomObjects)
                    {
                        // float distance = roomObject.distance(lastUserPosition);
                        // Truncate the number into a two decimals number
                        // distance = (float)Math.Truncate((double)distance * 100.0) / 100f;
                        // If the object is too close
                        //if (distance < warningDistance)
                        bool toClose = roomObject.toClose(warningDistance, this.lastUserPosition);

                        if (toClose)
                        {
                            String objectName = roomObject.Name;
                            float angle = roomObject.relativeAngle(lastUserPosition, movementVector);
                            if (onlyFront)
                            {
                                if (angle == 12)
                                {
                                    if (device)
                                        notifier.speak("You have a " + objectName + " in front");
                                    else
                                        this.sendInfoToDevice(ip, "You have a " + objectName + " in front");
                                }
                            }
                            else
                            {
                                if (device)
                                    notifier.speak("You have a " + objectName + " at " + angle);
                                else
                                    this.sendInfoToDevice(ip, "You have a " + objectName + " at " + angle);
                            }
                        }
                    }
                    // now the same with the doors
                    foreach (RoomDoor roomDoor in roomDoors)
                    {
                        float distance = roomDoor.distance(lastUserPosition);
                        // Truncate the number into a two decimals number
                        distance = (float)Math.Truncate((double)distance * 100.0) / 100f;
                        // If the door is too close
                        if (distance < warningDistance)
                        {
                            float angle = roomDoor.relativeAngle(lastUserPosition, movementVector);
                            if (onlyFront)
                            {
                                if (angle == 12)
                                {
                                    if (device)
                                        notifier.speak("You have a door at " + distance + " centimeters");
                                    else
                                        this.sendInfoToDevice(ip, "You have a door at " + distance + " centimeters");
                                }
                            }
                            else
                            {
                                if (device)
                                    notifier.speak("You have at " + angle + " a door at " + distance + " centimeters");
                                else
                                    this.sendInfoToDevice(ip, "You have at " + angle + " a door at " + distance + " centimeters");
                            }
                        }
                    }
                    return new System.Drawing.PointF(lastUserPosition.X, lastUserPosition.Y);
                }
                else
                    return new System.Drawing.PointF(-1f, -1f);
            }
            else
                return new System.Drawing.PointF(-1f,-1f);
        }
        /// <summary>
        /// Return the position associated to the user relative to the room axis.
        /// </summary>
        /// <param name="firstWiimoteId">NON-Permanent ID associated with the first wiimote</param>
        /// <param name="secondWiimoteId">NON-Permanent ID associated with the second wiimote</param>
        /// <param name="firstPosition">Position captured by the first wiimote</param>
        /// <param name="secondPosition">Position captured by the second wiimote</param>
        /// <returns></returns>
        public PointF userPosition(Guid firstWiimoteId, Guid secondWiimoteId, PointF firstPosition, PointF secondPosition)
        {
            // Identifying the RoomWiimote associate with the Wiimotelib.Wiimote
            RoomWiimote firstRoomWiimote = roomWiimotes[wiimotesIDs[firstWiimoteId]];
            RoomWiimote secondRoomWiimote = roomWiimotes[wiimotesIDs[secondWiimoteId]];

            // First, we use the coordinates from the different wiimotes to obtain
            // the angle between the wiimote's visibility border vector and the position vector.

            float firstPointAngle = positionToAngle(firstPosition.X);
            float secondPointAngle = positionToAngle(secondPosition.X);
            //Debug.WriteLine("FirstAngle: " + firstPointAngle + " SecondAngle: " + secondPointAngle);

            // Calculating the projection points for each Wiimote
            PointF firstProjectionPoint = firstRoomWiimote.projectionPoint(roomPoints, firstPointAngle);
            PointF secondProjectionPoint = secondRoomWiimote.projectionPoint(roomPoints, secondPointAngle);
            //Debug.WriteLine("FirstProjection: " + firstProjectionPoint+ " SecondProjection: " + secondProjectionPoint);
            
            // IN THIS FINAL PART THE ALGORITHM CALCULATE THE USER POSITION INTERSECTING THE STRAIGHT LINES
            // GENERATED BETWEEN THE WIIMOTE POINT AND ITS RESPECTIVE PROJECTION

            // calculating the slopes (Y2-Y1)/(X2-X1)
            float firstSlope = (firstProjectionPoint.Y - firstRoomWiimote.Position.Y) / (firstProjectionPoint.X - firstRoomWiimote.Position.X);
            float secondSlope = (secondProjectionPoint.Y - secondRoomWiimote.Position.Y) / (secondProjectionPoint.X - secondRoomWiimote.Position.X);

            // Position Calculation
            PointF userPosition = new PointF();
            userPosition.X = (secondSlope * secondRoomWiimote.Position.X -
                                firstSlope * firstRoomWiimote.Position.X +
                                firstRoomWiimote.Position.Y - secondRoomWiimote.Position.Y) / (secondSlope - firstSlope);

            userPosition.Y = secondRoomWiimote.Position.Y + secondSlope * (userPosition.X - secondRoomWiimote.Position.X);

            return userPosition;

        }
        /// <summary>
        /// Send the position info the the specified device
        /// </summary>
        /// <param name="ip">IP  Address associated with the mobile device</param>
        /// <param name="text">Text to be spoken by the device</param>
        private void sendInfoToDevice(String ip, String text)
        {
            string path = "ttstemp.wav";
            SapiTTS tts = new SapiTTS();
            tts.speakToStream(path, text);
            FileStream fileStream = new FileStream(path, FileMode.Open);
            //SoundPlayer soundPlayer = new SoundPlayer(fileStream);
            //soundPlayer.Play();
            Client.Connect(ip, fileStream);
            fileStream.Close();
        }
        private float positionToAngle(float wiimoteRelativePosition)
        {
            // Max angle reachable by the wiimote measure to the center
            float visualizationAngle = RoomWiimote.OpeningAngle;
            // Relationship between the angle and the segment
            // TODO: Check the relationship. For now is linear.
            // TODO: Check the increase direction
            return (2f * visualizationAngle) * wiimoteRelativePosition;
        }
    }
}
